Attitude alignment of a slave inertial measurement system

ABSTRACT

A method for attitude alignment of a slave inertial measurement system connected to a rotationally mobile platform supported by a vehicle that is stationary relative to a reference navigation frame comprises the steps of mounting a master reference inertial measurement system on the rotationally mobile platform and determining a master reference system attitude using measurements of acceleration and angular rates of the master reference inertial measurement system relative to the reference navigation frame. A slave system attitude is determined using measurements of acceleration and angular rates of the slave inertial measurement system relative to a slave system navigation reference frame and comparing the slave system attitude to the master reference system attitude to determine an attitude difference. The attitude difference is processed to obtain a correction to the slave system attitude.

CROSS REFERENCE TO RELATED APPLICATION

Applicants claim the benefit of U.S. Provisional Application Ser. No.60/221,315, filed Jul. 28, 2000 for Transfer Alignment Using AttitudeMatching And Velocity Observations On A Rotationally Mobile Platform.

Statement of Government Rights in Invention: The United StatesGovernment has rights in this invention under U.S. Army contract No.DAAH01-98-C0003.

BACKGROUND OF THE INVENTION

This invention relates generally to navigation equipment and techniques.This invention relates particularly to apparatus and methods forcorrecting attitude errors of a vehicle supported on a rotationallymobile platform such as a rocket that is to be launched from a pod thatis mounted to a vehicle.

SUMMARY OF THE INVENTION

This invention is directed to a transfer alignment system that usesattitude matching transfer align and velocity observations on arotationally mobile platform.

The method of the invention for attitude alignment of a slave inertialmeasurement system that is connected to a rotationally mobile platformthat is supported by a vehicle that is stationary relative to areference navigation frame, comprises the steps of mounting a masterreference inertial measurement system on the rotationally mobileplatform and determining a master reference system attitude usingmeasurements of acceleration and angular rates of the master referenceinertial measurement system relative to the reference navigation frame.The method further comprises the steps of determining a slave systemattitude by using measurements of acceleration and angular rates of theslave inertial measurement system relative to a slave system navigationreference frame and comparing the slave system attitude to the masterreference system attitude to determine an attitude difference. Theattitude difference is processed to obtain a correction to the slavesystem attitude.

The invention may include the sequence of events for boresightcalculation and requirements for the compensation mechanization,including initialization, transfer align quaternion message processing,quaternion averaging and zero velocity updates.

The invention may include the step of filtering the attitude differenceto provide an observation for a Kalman filter, which transfer aligns theslave system. Differencing of attitude is preferably accomplished via aquaternion mechanization that permits large angles to be accommodated.Filtering is preferably performed by conversion to a vector angle thatis more appropriate to filtering than would be a quaternion, a directioncosine matrix, or a set of Euler angles.

The boresight compensation may be conducted using reference inertialmeasurement unit (IMU) measurements compared to slave IMU sensor outputsand initial approximated boresight angles. Master reference body toreference navigation (NAV) frame quaternions are rotated by storedboresight quaternions and compared with slave body to navigation framequaternions. Each of the samples is compared, and an average differenceis calculated. The rotation vector corresponding to the quaternion“difference” for each sample is computed between corresponding masterreference quaternion and slave quaternion measurements. The threecomponents of the vector difference are then averaged. The averagerotation vector so obtained is processed into the Kalman filter. Thealgorithm provides correct timing alignment of the different quaternionsamples and the mechanization for calculating small angle rotationangles using quaternion measurements.

The invention also may include a method of performing “zero velocityupdates” on the slave system. A pseudo null velocity is created whichcan be used to improve the navigation axis alignment of the slave unit.This method is implemented without recourse to angular rates andaccelerations and, instead, uses a change in attitude to compensate forrotation-induced position change.

A velocity error is calculated using the measured velocity of the slaveIMU and the known change in position of the slave IMU in the NAV frame.This mechanization is a comparison of position change in the navigationframe of the slave system body. The calculated change in position issubtracted from the integrated navigation velocity over same known timeinterval. The result is the observation for the Kalman Filter states forvelocity error.

BRIEF DESCRIPTION OF THE DRAWINGS

FIG. 1 illustrates Cartesian coordinates of a master IMU reference body;

FIG. 2 illustrates Cartesian coordinates of a slave system body;

FIG. 3 illustrates Cartesian coordinates of a slave IMU orthogonalframe;

FIG. 4 illustrates Cartesian coordinates of an inertial navigation unit(INU) reference frame;

FIG. 5 illustrates the relationships of the slave system body frame, theslave system orthogonal frame and the slave system navigation referenceframe to the master reference body and master reference navigationframes and further shows the slave system body to master reference bodyboresight;

FIG. 6 shows the timing sequence for initializing axis transformationsused in implementing the present invention;

FIG. 7 illustrates a lever arm structure that conceptually illustratesthe invention;

FIG. 8 illustrates numbering of rockets in a pair of rocket pods for thepresent invention;

FIG. 9 is an events time line for the present invention; and

FIG. 10 is a software diagram for the present invention.

DETAILED DESCRIPTION OF THE INVENTION

This invention is directed to a transfer alignment mechanization usingattitude matching, transfer align and velocity observations on arotationally mobile platform 20, such as a rocket, as shown in FIG. 1mounted in a launching pod 22 or 24 as shown in FIG. 2. Referring toFIGS. 2 and 8, before launch, the rocket 20 ordinarily is contained in apod 22 that is mounted on a land-based vehicle 23 such as a truck or atank. FIGS. 2 and 8 show a second pod 24 with each of the pods 22 and 24containing a plurality of rockets 20. The pods 22 and 24 are normallykept in a travel lock (TL) position except during the launch sequence.Controlling the rocket 20 involves aligning the rocket to a stablenavigation reference frame. This is accomplished using a master inertialmeasurement unit (IMU) 26 fixed to the vehicle 23 to serve as areference body and the axes of a slave IMU 28 fixed to the rocket 20 toserve as a slave system body. However, an additional unknown is therelative alignment of the master IMU 26 axes and the slave IMU 28 axes.This relative alignment is known as the boresight. Because the rocket 20is expendable, the slave sensor 28 typically is less accurate than thereference sensor 26, which sometimes results in errors in alignment Thisinvention corrects the boresight error and aligns the slave IMU 28 to areference navigation frame before the rocket 20 is launched.

Some of the calculations necessary for compensating for boresight errorsare performed more conveniently using quaternions instead of the moreconventional vectors. Quaternions are conveniently used in calculationsthat involve transformations between coordinate systems that rotaterelative to one another. A brief explanation of the basic properties ofquaternions is presented to facilitate understanding the presentinvention. Quaternions are the result of a generalization of the complexnumber system. In the complex number system there are threeanti-commuting hypercomplex roots of −1, which in quaternion notationare represented by the letters i, j, k, where. The numbers i, j, k, havethe following properties:i*j=−j*i=k  (1)j*k=−k*j=i  (2)k*i=−i*k=j  (3)

A quaternion may be written as linear combination of a number w and theproduct of coefficients x, y, z and i, j, k, which may be written as:q=w+xi+yj+zk.  (4)The conjugate of a quaternion q is written as:q*=w−xi−yj−zk.  (5)A quaternion q has a magnitude that is written as∥q∥=√{square root over (qq*)}=√{square root over (w² +x ² y ² +z²)}.  (6)A unit quaternion has a magnitude of 1, therefore for a unit quaternionq⁻¹=q* where q⁻¹ is the multiplicative inverse, which can be computed by

$\begin{matrix}{q^{- 1} = {\frac{q^{*}}{q\; q^{*}}.}} & (7)\end{matrix}$Quaternions are associative such that(q ₁ *q ₂)*q ₃ =q ₁*(q ₂ *q ₃).  (8)However, quaternions are not commutative such thatq ₁ *q ₂ ≠q ₂ *q ₁.  (9)

In addition to the expression of Eq. (4) a quaternion q may be written arow vector, a column vector or as a combination of a vector and ascalar:

$\begin{matrix}{{q = {\begin{bmatrix}x & y & z & w\end{bmatrix} = {\lfloor \begin{matrix}x \\y \\z \\w\end{matrix} \rfloor = ( {s,v} )}}},} & (10)\end{matrix}$where s=w and v=[x y z]. The product of two quaternions q₁=(s₁,v₁) andq₂=(s₂,v₂) can be written using the multiplicative properties of i, j,k, and standard vector products in the following way:q ₁ q ₂=(s ₁ s ₂ −v ₁ ·v ₂ , S ₁ v ₂ +s ₂ v ₁ +v ₁ ×v ₂).  (11)

The quaternion that represents rotation, or orthogonal transformation,about a unit vector u by an angle θ may be written as:

$\begin{matrix}{q = {( {s,v} ) = {( {{\cos\frac{\theta}{2}},{u\;\sin\frac{\theta}{2}}} ).}}} & (12)\end{matrix}$A point p in space may be represented by the quaternion P=(0, p).Rotation of the point p about a unit vector u is computed as follows:P _(rotated) =qPq ⁻¹.  (13)Some calculations associated with the present invention involve tworotations. If q₁ and q₂ are quaternions representing two rotations, acomposite rotation that comprises the first rotation followed by thesecond rotation is represented by the quaternion q₂q₁. Using Eq. (13)the result of applying the composite rotation to the point P is writtenas:

$\begin{matrix}{{{q_{2}( {q_{1}P\; q_{1}^{- 1}} )}q_{2}^{- 1}} = {{( {q_{2}q_{1}} ){P( {q_{1}^{- 1}q_{2}^{- 1}} )}^{- 1}} = {( {q_{2}q_{1}} ){{P( {q_{1}q_{2}} )}^{- 1}.}}}} & (14)\end{matrix}$

The product of two quaternionsq ₁ =w ₁ +x ₁ i+y ₁ j+z ₁ k.  (15)q ₂ =w ₂ +x ₂ i+y ₂ j+z ₂ k  (16)can be written as

$\begin{matrix}\begin{matrix}{{q_{1}q_{2}} = {{w_{1}w_{2}} - {x_{1}x_{2}} - {y_{1}y_{2}} - {z_{1}z_{2}} +}} \\{{( {{w_{1}x_{2}} + {x_{1}w_{2}} + {y_{1}z_{2}} - {z_{1}y_{2}}} )i} +} \\{{( {{w_{1}y_{2}} - {x_{1}z_{2}} + {y_{1}w_{2}} + {z_{1}x_{2}}} )j} +} \\{( {{w_{1}z_{2}} + {x_{1}y_{2}} - {y_{1}x_{2}} + {z_{1}w_{2}}} )k}\end{matrix} & (17)\end{matrix}$Rotations may also be computed using matrices. However, a matrix productrequires many more mathematical operations than a quaternion product.Therefore using quaternions saves time while preserving numericalaccuracy in such calculations.

This invention includes a sequence of events for the boresightcalculation and the compensation mechanization, which includeinitialization, transfer align quaternion message processing, quaternionaveraging, and zero velocity updates.

FIG. 3 shows an inertial measurement unit (IMU) 30, which includes athree-axis rotation sensor (not shown) and accelerometers (not shown)for providing angular orientation and acceleration for the rocket 20.The IMU 30 may be formed in accordance with well-known rotation sensorand accelerometer technology. FIG. 4 shows the IMU 30 coupled to aglobal positioning system (GPS) module 32 to form an inertial navigationunit (INU) 33 that provides data on the angular orientation,acceleration, velocity and position of the rocket 20. GPS modulessuitable for practicing the invention are well known in the art. The IMU30 includes an elongated connector 34 that may be used for coupling theGPS module 32 to the IMU 30.

As shown in FIG. 5, a system navigation (NAV) frame is defined as X_(N)and Y_(N) in the local level with wander angle azimuth α=0° (+X_(N)pointing north) and +Z_(N) pointing down. There are five referenceframes associated with boresight correction: (1) a master IMU referencebody frame, (2) a reference navigation (NAV) frame, (3) a slave systembody frame, (4) a slave system NAV frame and (5) a slave systemorthogonal frame. The master IMU reference body axes (X_(RB), Y_(RB),Z_(RB)) are shown in FIG. 2, with +X_(RB) pointing forward, +Z_(RB)pointing down, and +Y_(RB) perpendicular to X_(RB) and Z_(RB) forming aright-handed reference frame. The slave system body frame axes (X_(B),Y_(B), Z_(B)) are shown in FIG. 1, and are +X_(B) out of the nose of therocket 20, +Y_(B) out the starboard side, and +Z_(B) out the belly. Theslave system reference frame axes (X, Y, Z) are shown in FIG. 3 with +Xbeing along the connector 34, +Y being perpendicular to the connector 34and +Z being upwards.

FIG. 4 shows the slave system orthogonal frame. This frame ismathematically related to the slave system body frame using apredetermined relationship. FIG. 5 shows the relationship of the slavesystem body, slave system orthogonal, and slave system NAV frames to themaster reference body and master reference NAV frames. This diagram alsoshows the slave system body to reference body boresight. In FIG. 5 thefollowing definitions apply:

C_(RN)^(RB)is the direction cosine matrix indicating rotation from the masterreference NAV frame to the master reference body frame and is availableas the quaternion

Q_(RN)^(RB).

C_(N) ^(O) is the direction cosine matrix indicating rotation from theslave system NAV frame to the slave system orthogonal frame and isavailable as the quaternion Q_(N) ^(O).

C_(O) ^(B) is the direction cosine matrix indicating rotation from theslave system orthogonal frame to the slave system body frame(predetermined relationship) and is available as the quaternion Q_(O)^(B).

C_(RB)^(B)is the direction cosine matrix indicating rotation from the masterreference body frame to the slave system body frame(boresight)—(unknown). (Also denoted by the quaternion representation

q_(L → B)  or  Q_(RB)^(B)).

C_(RN)^(N)is the direction cosine matrix indicating rotation from the referenceNAV frame to the slave system NAV frame.If the reference NAV frame tilts are zero and

α = α_(R) = 0, then  C_(RN)^(N)corresponds to the rotation due to system NAV tilts φ_(x) and φ_(y) andthe azimuth rotation φ_(z).

There are five major events in the prelaunch sequence, which occurbefore the rocket 20 will be launched. These are described below and areshown in the events time line of FIG. 9, which is not drawn to scale.The first event is Power On (PO), which initializes execution of thesequence.

The second event is a launcher initialization message that includes anInitialize Data Command 40 shown in the flow diagram of FIG. 10. TheInitialize Data Command 40 initializes a master reference body frame toMaster reference NAV frame quaternion 42 and initializes a masterreference body to slave system body frame quaternion 44, which representa priori knowledge of the latitude, longitude, height, ground aligncommand, orientation and boresight of a selected bore in the rocket pod20. The Intialize Data Command 40 also initializes the slave systemorthogonal frame to the NAV frame quaternion 46 and initializes theslave orthogonal frame to the slave system body frame quaternion 48quaternion q_(o→B). Travel lock (TL) release will occur after Power Onand before movement to the first position. The initialization messagecan occur at any time during this period. The timing sequence for theinitialization processes is shown in FIG. 6.

The third event is setting the first alignment position, which is theposition of the selected rocket pod 22 after travel locks have beenreleased and the pod 22 is stabilized.

The fourth event is when the rocket pod 22 moves to its second alignmentposition after indication of receipt of position 1 information from themaster reference system.

The fifth event occurs when the rocket pod 22 moves to position 3 afterreceipt of position 2 information from the master reference system.Position 3 is typically a launch angle that is 20° to 60° elevationabove local level and 90° to 180° azimuth from the initial position.After the receipt of this last matching message from the master system,the rocket 20 is ready for launch.

At each of the first three initialization positions, the rocket 20preferably receives 2 seconds of 10 Hz data, which corresponds to 20quaternion samples. Updates will continue at the launch position untilall the rockets have been fired or until the rocket pod 22 is re-stowedand the power is turned off. The master INS preferably provides aquaternion message of 1 second of 10 Hz data every 30 seconds forperiodic updates. If power is cycled, the rockets will be reinitialized.

Slave system orthogonal to slave system body initialization 48 iscontrolled by the algorithm, which polls for an initialization messageflag at 200 Hz. Quaternion initialization utilizes the master reference(launcher) frame to the master reference geodetic frame quaternion(q_(L→Geo)) 42 and the master reference frame to slave system framequaternion (q_(L→B)) 44 data received from the initialize data command40. The master reference body initialization quaternion (q_(L→Geo)) 42,initial master reference body to slave system quaternion (q_(B→L)) 44and slave system orthogonal to slave system body quaternion (q_(O→B)) 48initialize the slave system orthogonal frame to the slave system NAVframe quaternion (q_(O→N)) 52 as shown below. For initialization andtransfer align, α_(ref)=0, thus initially the slave system orthogonalframe to the slave system NAV frame quaternion (q_(0→N)) equals theslave system orthogonal frame to geodetic frame quaternion.

The following equation applies:q _(0→N) =q _(0→Geo) =q _(L→Geo) q* _(L→B) q _(0→B)  (18)where

q_(0→Geo)=slave system orthogonal frame to geodetic frame quaternion(NAV frame when α=0);

q_(L→Geo)=master reference body frame to geodetic frame quaternion;

q_(L→B)=master reference body frame to slave system body framequaternion (boresight);

q_(0→B)=slave system orthogonal frame to slave system body framequaternion; and

q* is the inverse quaternion, which is achieved by negating the vectorpart of a rotation quaternion.

An arbitrary vector V expressed in the slave system frame is transformedto the system NAV frame by:V ^((N)) =q _(B→N) V ^((B)) q* _(B→N).  (19)This concludes the initialization processes.

Quaternion message processing includes periodically updating thequaternions. The quaternion updates are performed upon receipt of themaster reference body frame to geodetic frame sets of quaternions. Thereare either 10 or 20 sets of quaternions depending on the transfer alignmode. For the twenty-sample mode, the processing algorithm is run aswith 10 samples, but twice consecutively, shifting the buffer pointerand word address.

A buffering process buffers the slave system quaternions internally.This buffering preferably is at a frequency of 200 Hz. These are theslave system body frame to slave system NAV frame quaternions (q_(Ba))as computed by the IMU. This data is buffered using the index calculatedbelow:

At 200 Hz

If (++L_(200 Hzcnt) ==interval)

{L_(200 Hzcnt)=0

q_(index)++

q_(index) &=(bufflength−1)

q_(buff)[q_(index)]=q_(Ba)}

if (update message is received)

{q_(indexlatch)=q_(index)

Latch_(200 Hzcnt)=L_(200 Hzcnt)

where:

bufflength=512 (size of buffer, must be a power of 2);

interval=1;

q_(Ba)=slave system body to slave system NAV system output quaternion;and

L_(200 Hzcnt)=200 Hz counter.

The IMU software uses the index calculated above to extract the centerslave body NAV quaternion from the buffered data. The direction cosinesC_(N) ^(B) required for the measurement matrix H of the Kalmanprocessing are determined using this sample by conversion of the centerquaternion to a direction cosine matrix.

This section provides an analysis that will be referenced subsequentlyfor quaternion processing and rotation vector averaging. The masterreference system quaternions q_(L→Geo) are rotated by the storedboresight quaternions q_(BOR) to provide a reference quaternion 56. Thisrotation is accomplished by multiplying the quaternion q_(L→Geo) by thequaternion q_(BOR). The result of the rotation is compared with slavesystem body quaternions. All of the samples (20 or 10) are compared andan average difference is then calculated. If the quaternions themselvesare directly averaged, the four averaged components may no longerrepresent a valid normalized quaternion. Instead, the rotation vectorcorresponding to the quaternion “difference” for each sample is computedbetween corresponding master reference quaternion and slave systemquaternion measurements. The three components of the vector differenceare then averaged in a Calculate Component Average process 60. Theaverage rotation vector so obtained is processed into the Kalman filter62.

A reference quaternion q_(B) is defined as the master reference bodyframe to master reference NAV frame quaternion multiplied with the slavesystem body frame quaternion to the master reference body frameboresight quaternion, which may be written asq _(B) =q _(L) q* _(BOR).  (20)For compactness the master reference body frame to geodetic framequaternion q_(L→Geo) is written as:q _(L) =q _(L→Geo);  (21)The master reference body frame to slave system body frame quaternionq_(L→B) (boresight) is written as:q _(BOR) =q _(L→B).  (22)

Recalling that q_(Ba)=slave system body frame to slave system NAV framequaternion (approximation of q_(B)), the small angle approximation forthe rotation vector is:(φ,0)_(ε) ≈q _(Ba) q* _(Ba) −q _(B) q* _(Ba)  (23)φ_(ε)≈2*vector{q_(Ba)q*_(B)}  (24)Using Eq. (20), the quaternion product q_(Ba)q_(B)* in Eq. (24) may bewritten asq _(Ba) q* _(B) =q _(Ba) q _(BOR) q* _(L).  (25)

The following steps may be used to implement Eqs. (23), (24) and (25).Other orders of processing are possible.

Step 1: Determine the quaternion product q_(BOR)q_(L)* (both scalar andvector are needed). The quaternions q_(BOR) and q_(L) may be written asq_(BOR)=(q_(BOR1),q_(BOR2),q_(BOR3),q_(BOR4)),  (26)andq_(L)=(q_(L1),q_(L2),q_(L3),q_(L4)).  (27)In Eqs. (26) and (27) the subscripts 1, 2, 3 correspond to i, j, k andthe subscript 4 refers to the scalar components of the quaternions. Themaster reference quaternion q_(L) is the master reference frame tomaster reference NAV frame (geodetic frame) quaternion and is sentduring each update (either 10 or 20). Now define a quaternion q_(F) asfollows:q* _(B) =q _(BOR) q* _(L) =q _(F)=(q _(F1) ,q _(F2) ,q _(F3) ,q_(F4))  (28)The components of q_(F4), q_(F1), q_(F2) and q_(F3) may now becalculated as follows using quaternion multiplication beginning with thescalar term q_(F4):q _(F4)=(q _(BOR4) q _(L4) +q _(BOR1) q _(L1) +q _(BOR2) q _(L2) +q_(BOR3) q _(L3))(scalar)  (29)q _(F1)=(−q _(BOR4) q _(L1) +q _(BOR1) q _(L4) −q _(BOR2) q _(L3) +q_(BOR3) q _(L2))  (30)q _(F2)=(−q _(BOR4) q _(L2) +q _(BOR2) q _(L4) −q _(BOR3) q _(L1) +q_(BOR1) q _(L3))  (31)q _(F3)=(−q _(BOR4) q _(L3) +q _(BOR3) q _(L4) −q _(BOR1) q _(L2) +q_(BOR2) q _(L1))  (32)This multiplication is done for ten samples of q_(F) (1 through 10)using the same characteristic boresight q_(BOR) for all ten samples. Theresult is:q_(Fj)=q_(Fj1),q_(Fj2),q_(Fj3),q_(Fj4) where j=(1, . . . , 10).  (33)

Step 2: Using Eq. (24):φ_(ε)=2*vector(q_(Ba)q_(F))  (34)φ_(εx)=2(q _(Ba4) q _(F1) +q _(Ba1) q _(F4) +q _(Ba2) q _(F3) −q _(Ba3)q _(F2))  (35)φ_(εy)=2(q _(Ba4) q _(F2) +q _(Ba2) q _(F4) +q _(Ba3) q _(F1) −q _(Ba1)q _(F3))  (36)φ_(εz)=2(q _(Ba4) q _(F3) +q _(Ba3) q _(F4) +q _(Ba1) q _(F2) −q _(Ba2)q _(F1)).  (37)This multiplication is done for ten time-aligned samples of q_(Ba) andq_(F).

Now the average values of φ_(εx), φ_(εy) and φ_(εz) are calculated.These averages may be written as:

$\begin{matrix}{{\overset{\_}{\phi}}_{ɛ\; x} = {\frac{1}{10}{\sum\limits_{i = 1}^{10}\;\phi_{ɛ\; x\; i}}}} & (38) \\{{\overset{\_}{\phi}}_{ɛ\; y} = {\frac{1}{10}{\sum\limits_{i = 1}^{10}\;\phi_{ɛ\; y\; i}}}} & (39) \\{{\overset{\_}{\phi}}_{ɛ\; z} = {\frac{1}{10}{\sum\limits_{i = 1}^{10}\;{\phi_{ɛ\; z\mspace{11mu} i}.}}}} & (40)\end{matrix}$

The average rotation angles for the three vector components of theboresight error are applied as observations to the Kalman filter.

$\begin{matrix}{\begin{bmatrix}{\overset{\_}{\phi}}_{ɛ\; x} \\{\overset{\_}{\phi}}_{ɛ\; y} \\{\overset{\_}{\phi}}_{ɛ\; z}\end{bmatrix} = {\begin{bmatrix}1 & 0 & 0 & ( {- C_{N}^{B}} )_{11} & ( {- C_{N}^{B}} )_{12} & ( {- C_{N}^{B}} )_{13} \\0 & 1 & 0 & ( {- C_{N}^{B}} )_{21} & ( {- C_{N}^{B}} )_{22} & ( {- C_{N}^{B}} )_{23} \\0 & 0 & 1 & ( {- C_{N}^{B}} )_{31} & ( {- C_{N}^{B}} )_{32} & ( {- C_{N}^{B}} )_{33}\end{bmatrix}\begin{bmatrix}\begin{matrix}\begin{matrix}\begin{matrix}\begin{matrix}\varphi_{\chi} \\\varphi_{\gamma}\end{matrix} \\\varphi_{z}\end{matrix} \\{\delta\; b\; s_{\chi}}\end{matrix} \\{\delta\; b\; s_{\gamma}}\end{matrix} \\{\delta\; b\; s_{z}}\end{bmatrix}}} & (41)\end{matrix}$The measurement noise for the X, Y and Z observations is preselectedappropriately for the application in question.

The boresight observation and error states for the tilt and boresightare written as:

Observation: States: $\begin{bmatrix}{\overset{\_}{\phi}\;}_{ɛx} \\{\overset{\_}{\phi}\;}_{ɛy} \\{\overset{\_}{\phi}\;}_{ɛz}\end{bmatrix}\quad$ $\begin{bmatrix}\varphi_{x} \\\varphi_{y} \\\varphi_{z} \\{\delta bs}_{x} \\{\delta bs}_{y} \\{\delta bs}_{z}\end{bmatrix}\mspace{34mu}\begin{matrix}{\}\begin{matrix}\; \\{Tilt} \\\;\end{matrix}} \\{\}\begin{matrix}\; \\{Boresight} \\\;\end{matrix}}\end{matrix}$

The initial covariances and process noise for the three boresight statesare determined by the uncertainty in the mechanical alignment betweenthe master and slave systems. The Transition Matrix portion for thethree boresight states is:

$\begin{bmatrix}1 & 0 & 0 \\0 & 1 & 0 \\0 & 0 & 1\end{bmatrix}$

The three small angle rotations that are applied to the presentboresight rotation and used to form the boresight update quaternion are:

$\begin{matrix}{( Q_{\delta\; b\; s} )_{1} = \frac{\delta\; b\; s_{x}}{2}} & (42) \\{( Q_{\delta\; b\; s} )_{2} = \frac{\delta\; b\; s_{y}}{2}} & (43) \\{( Q_{\delta\; b\; s} )_{31} = \frac{\delta\; b\; s_{z}}{2}} & (44) \\{( Q_{\delta\; b\; s} )_{4} = {1 - {\frac{1}{2}{\{ {\lbrack ( Q_{\delta\; b\; s} )_{1} \rbrack^{2} + {\lbrack ( Q_{\delta\; b\; s} )_{2} \rbrack^{2}\lbrack ( Q_{\delta\; b\; s} )_{3} \rbrack}^{2}} \}.}}}} & (45)\end{matrix}$

The boresight update quaternion is used to improve the currentquaternion approximation. This will form the new approximation, whichis:

$\begin{matrix}{( Q_{RB}^{B} )_{4{({n + 1})}} = {{( Q_{\delta\; b\; s} )_{4}( Q_{RB}^{B} )_{4{(n)}}} - {( Q_{\delta\; b\; s} )_{1}( Q_{RB}^{B} )_{1{(n)}}} - \mspace{146mu}{( Q_{\delta\; b\; s} )_{2}( Q_{RB}^{B} )_{2{(n)}}} - {( Q_{\delta\; b\; s} )_{1}( Q_{RB}^{B} )_{1{(n)}}}}} & (46) \\{( Q_{RB}^{B} )_{3{({n + 1})}} = {{( Q_{\delta\; b\; s} )_{4}( Q_{RB}^{B} )_{3{(n)}}} + {( Q_{\delta\; b\; s} )_{3}( Q_{RB}^{B} )_{4{(n)}}} + \mspace{146mu}{( Q_{\delta\; b\; s} )_{1}( Q_{RB}^{B} )_{2{(n)}}} - {( Q_{\delta\; b\; s} )_{2}( Q_{RB}^{B} )_{1{(n)}}}}} & (47) \\{( Q_{RB}^{B} )_{1{({n + 1})}} = {{( Q_{\delta\; b\; s} )_{4}( Q_{RB}^{B} )_{1{(n)}}} + {( Q_{\delta\; b\; s} )_{1}( Q_{RB}^{B} )_{4{(n)}}} + \mspace{146mu}{( Q_{\delta\; b\; s} )_{2}( Q_{RB}^{B} )_{3{(n)}}} - {( Q_{\delta\; b\; s} )_{3}( Q_{RB}^{B} )_{2{(n)}}}}} & (48) \\{( Q_{RB}^{B} )_{2{({n + 1})}} = {{( Q_{\delta\; b\; s} )_{4}( Q_{RB}^{B} )_{2{(n)}}} + {( Q_{\delta\; b\; s} )_{2}( Q_{RB}^{B} )_{4{(n)}}} + \mspace{146mu}{( Q_{\delta\; b\; s} )_{3}( Q_{RB}^{B} )_{1{(n)}}} - {( Q_{\delta\; b\; s} )_{1}( Q_{RB}^{B} )_{3{(n)}}}}} & (49)\end{matrix}$

Periodic quaternion updates continue until the rocket 20 on the pod 22is launched. After the rocket 20 is launched, and the resulting motionof the pod 22 has been damped, the rockets remaining in the pod 22 willreceive a master reference quaternion message that will include a“Rocket Fired” flag. When this flag is detected, the diagonals of thethree boresight states covariances are forced to increase (Q pumped) toindicate a possible boresight shift. The three diagonals of theboresight states covariances will be incremented by ΔP, such that thenew covariance P_(new)=P+ΔP. The off-diagonal elements will not bechanged. The covariance increase must be done before the quaternionmessage is processed. ΔP will be determined by the anticipated boresightshift due to launch dynamics. New master reference quaternions are madeavailable at the first update period after each launch. Quaternionobservations will quickly capture the new boresight errors. Thisconcludes the transfer align process.

In order to further enhance performance, a second aspect of the presentinvention performs pseudo zero velocity updates. The invention adds anew step to a standard zero velocity updating scheme. The firstcalculation of the zero velocity updating mechanization computes V_(i)(velocity vector in the NAV frame), which is assumed to be zero forstandard zero velocity updating processing. For use on a rotationallymobile platform such as a gimbaled launcher, it is recognized that theslave system may experience a non-zero velocity in the NAV frame even ifthe vehicle itself is stationary. This non-zero velocity results fromrotational motion of the launcher pods 22 and 24. A correction to theslave velocity is computed to form a pseudo zero velocity. This velocityerror is calculated by using the measured velocity of the slave systemIMU and the known change in position of the slave system body IMU in theNAV frame using lever arms of the IMU position and pivot points.

FIG. 7 illustrates IMU position (point B), Azimuth Pivot (point 0),Elevation Pivot (point P), and conceptual lever arms L₁, L₂, L₃, and L₄that are useful in understanding the present invention.

The lever arms L₁ and L₃ have ends 35 and 36, respectively, thatintersect at a right angle. The other end 37 of the lever arm L₃ isarranged to be pivotable about the elevation pivot axis P, shown atEL=0. The lever arm L, has an end 38 that bisects the lever arm L₂ at aright angle. The slave system IMU position, point B, is at an end 39 ofthe lever arm L₂. The coordinates for the IMU are shown at point B withaxes X_(G) pointing to the right, Z_(G) pointing downward and Y_(G)being perpendicular to X_(G) and Y_(G) and arranged to form a right handcoordinate system. The lever arm L₄ extends from the elevation pivotaxis to point O, which is on the azimuth pivot axis. A coordinate systemX_(T), Y_(T) and Z_(T) is centered at point O. The Z_(T) axis extendsdownward and defines the azimuth pivot axis. The X_(T) points to theright, and the Y_(T) axis points forward.

This mechanization is a comparison of position change in the navigationframe of the center of navigation of the slave system IMU over the1-second time interval synchronized with the Kalman filter. This will beused in measurement observation. This is due to the motion of thelauncher azimuth and elevation gimbals.

The following assumptions are made:

The elevation pivot axis hole, located at point P on the elevationstructure, lies parallel to the Y_(G) axis.

The elevation pin (located at point P on the turret structure) islocated on the negative X_(T) axis.

The elevation pin lies in the X_(T) Y_(T) plane, pointing in the Y_(T)direction.

The following notation is used for the direction cosine matrices:

C_(N 1)^(B)

-   -   is the system direction cosine matrix, NAV frame to system body        frame, at the start of the 1-second interval.

C_(N 2)^(B)

-   -   is the system direction cosine matrix, NAV frame to slave system        body frame, at the end of the 1-second interval.

C_(RB)^(B)

-   -   is the direction cosine matrix of rotation from the master        reference body frame to the slave system body frame. This matrix        represents a frame rotation due to total boresight.

The change in position is calculated by solving for the change of theconceptual lever arm between points O and B (L_(OB)). The equations forthe calculation are:

$\begin{matrix}{D = {( C_{RB}^{B} )^{T}{{C_{N\; 2}^{B}\lbrack {( C_{RB}^{B} )^{T}C_{N\; 1}^{B}} \rbrack}^{T}.}}} & (50)\end{matrix}$The elements of D^(T) are used to solve for the velocity effect on thelever arm L₄ (VEC). If (D^(T))₂₂<0.99955 then VEC is calculated asfollows:

$\begin{matrix}{{EL}_{1} = {T\;{AN}^{- 1}\lfloor \frac{- ( D^{T} )_{32}}{- ( D^{T} )_{12}} \rfloor}} & (51) \\{{SEL}_{1} = {{SI}\;{N( {EL}_{1} )}}} & (52) \\{{CEL}_{1} = \sqrt{1 - ( {SEL}_{1} )^{2}}} & (53) \\{{C\;\Delta\;\theta\; Z} = ( D^{T} )_{22}} & (54) \\{{S\;\Delta\;\theta\; Z} = \frac{- ( D^{T} )_{12}}{{CEL}_{1}}} & (55) \\{{VEC} = \begin{bmatrix}\begin{matrix}{{CEL}_{1}( {{C\;\Delta\;\theta\; Z} - 1} )} \\{S\;{\Delta\theta}\; Z}\end{matrix} \\{{SEL}_{1}( {{C\;\Delta\;\theta\; Z} - 1} )}\end{bmatrix}} & (56)\end{matrix}$If (D^(T))₂₂ is >0.99955 then VEC is calculated as follows:|SΔθZ|=√{square root over ([(D ^(T))₁₂]²+[(D ^(T))₂₂]²)}{square rootover ([(D ^(T))₁₂]²+[(D ^(T))₂₂]²)}  (57)If (D^(T))₁₂<0 thenSΔθZ=−|SΔθZ|  (58)ElseSΔθZ=|SΔθZ|  (59)

$\begin{matrix}{{VEC} = \begin{bmatrix}\begin{matrix}0 \\{S\;{\Delta\theta}\; Z}\end{matrix} \\0\end{bmatrix}} & (60)\end{matrix}$

After VEC is determined, the L_(OB) vector is solved as follows:

$\begin{matrix}{\begin{bmatrix}{( {LOB}_{2} )_{XN} - ( {LOB}_{1} )_{XN}} \\{( {LOB}_{2} )_{YN} - ( {LOB}_{1} )_{YN}} \\{( {LOB}_{2} )_{ZN} - ( {LOB}_{1} )_{ZN}}\end{bmatrix} = {{C_{N2}^{B}{C_{RB}^{B}\begin{bmatrix}L_{1} \\L_{2} \\L_{3}\end{bmatrix}}} - {C_{N1}^{B}C_{RB}^{B}\{ {\begin{bmatrix}L_{1} \\L_{2} \\L_{3}\end{bmatrix} + \begin{matrix}\begin{matrix}{{VEC}_{1}*L_{4}} \\{{VEC}_{2}*L_{4}}\end{matrix} \\{{VEC}_{3}*L_{4}}\end{matrix}} \}}}} & (61)\end{matrix}$LOB is the change in position over the 1-second interval. This is to bedifferenced from the measured velocity for each axis. This differencewill be the observation for the following Kalman Filter states:

$\begin{matrix}{\begin{bmatrix}{\delta\; V_{x}} \\{\delta\; V_{y}} \\{\delta\; V_{z}}\end{bmatrix} = {\begin{bmatrix}V_{X}^{N} \\V_{Y}^{N} \\V_{Z}^{N}\end{bmatrix} - \begin{bmatrix}{( {LOB}_{2} )_{XN} - ( {LOB}_{1} )_{XN}} \\{( {LOB}_{2} )_{YN} - ( {LOB}_{1} )_{YN}} \\{( {LOB}_{2} )_{ZN} - ( {LOB}_{1} )_{ZN}}\end{bmatrix}}} & (62)\end{matrix}$

The structures and methods disclosed herein illustrate the principles ofthe present invention. The invention may be embodied in other specificforms without departing from its spirit-or essential characteristics.The described embodiments are to be considered in all respects asexemplary and illustrative rather than restrictive. Therefore, theappended claims rather than the foregoing description define the scopeof the invention. All modifications to the embodiments described hereinthat come within the meaning and range of equivalence of the claims areembraced within the scope of the invention.

1. A method for attitude alignment of a slave inertial measurementsystem that is connected to a rotationally mobile platform that issupported by a vehicle that is stationary relative to a referencenavigation frame, comprising the steps of: mounting a master referenceinertial measurement system on the rotationally mobile platform;determining a master reference system attitude by forming a masterreference body to geodetic frame quaternion that indicates angulardisplacement between the master reference inertial measurement systemand the reference navigation frame; determining a slave system attitudeby forming a slave system body quaternion that indicates angulardisplacement between the slave system body frame and the slave systemnavigation reference frame; comparing the slave system attitude to themaster reference system attitude to determine an attitude difference;and processing the attitude difference to obtain a correction to theslave system attitude.
 2. The method of claim 1, further comprising aninitialization step that includes the steps of: obtaining an initialvalue of the master reference system attitude; obtaining an initialvalue of the slave system attitude; sampling the master reference systemattitude; sampling the slave system attitude; and processing the sampledmaster reference system attitude and the sampled slave system attitudeto obtain an attitude correction; and applying the attitude correctionto the slave system attitude to align the slave inertial measurementsystem.
 3. The method of claim 1 wherein the step of determining themaster reference system attitude further comprises the steps of:defining a master reference system body frame for the master referenceinertial measurement system; defining a slave system body frame for theslave inertial measurement system; and determining angular displacementsbetween the master reference system body frame and the slave system bodyframe.
 4. The method of claim 1, further comprising a step ofdetermining values of angular displacement between the slave inertialmeasurement system and the master reference inertial measurement system.5. The method of claim 1 wherein the reference navigation frame is ageodetic coordinate frame.
 6. The method of claim 1 wherein the step ofcomparing the slave system attitude to the master reference systemattitude comprises the steps of: forming a boresight quaternion thatindicates an initial value of angular displacement between the masterreference body frame and the slave system body frame; forming areference quaternion that combines the angular displacement of theboresight quaternion and that of the master reference body to referencenavigation frame quaternion; and calculating a rotation vector thatindicates rotation from the slave system body quaternion and thereference quaternion.
 7. The method of claim 6, further comprising thesteps of: collecting a selected number of sets of samples of the slavesystem body quaternion and the reference quaternion; calculating arotation vector for each set of samples; and calculating averagecomponent values for the rotation vector for the selected number ofsamples.
 8. The method of claim 7, further comprising the steps of:filtering the average component values with a Kalman filter to calculatea boresight update quaternion; and using the boresight update quaternionto calculate an updated boresight quaternion.
 9. The method of claim 1,further comprising the steps of: determining a velocity vector for theslave inertial measurement system; determining a position change of theslave inertial measurement system in the slave system navigationreference frame; and calculating a velocity error using the slave systemvelocity vector and the position change of the slave inertialmeasurement system in the slave system navigation reference frame.
 10. Amethod for correcting attitude errors of a unit that is to be launchedfrom a rotationally mobile platform mounted on a stationary launchvehicle, the unit including a slave sensor fixed thereto to serve as aslave system body frame and the rotationally mobile platform including amaster inertial measurement unit fixed thereto to serve as a masterreference body frame, comprising the steps of: initializing a boresightquaternion q_(BOR) that has initial values of angular displacementsbetween the master reference body frame and the slave system body frame;initializing latitude, longitude, height and angular orientation of aselected bore in the launch vehicle from which the unit is to belaunched; initializing a quaternion q*_(L→Geo) that indicates rotationfrom the master reference body frame to a geodetic frame; combining theboresight quaternion q_(BOR) with the quaternion q*_(L→Geo) to provide areference quaternion q*_(B) that indicates rotations between theselected bore and a slave system navigation frame; providing a slavesystem body quaternion that indicates rotation between the slave systembody frame and the slave system navigation frame; calculating an updatequaternion for adjusting the boresight quaternion thereby providing anupdated reference quaternion; and combining the slave system bodyquaternion and the updated reference quaternion to obtain a correctedattitude by correcting errors in the latitude, longitude, height andangular orientation of selected bore that have occurred betweeninitialization and launch.
 11. The method of claim 10 wherein the stepof combining the boresight quaternion with the quaternion q*_(L→Geo) toprovide a reference quaternion comprises the step of multiplying thequaternion q*_(L→Geo) by the boresight quaternion q_(BOR).
 12. Themethod of claim 11 wherein the step of calculating an update quaternioncomprises the steps of: multiplying the reference quaternion by theslave system body quaternion to obtain approximate values for errors inan the angular displacement between master reference body frame and theslave system body frame; calculating average values of the errors in theangular displacement for a selected number of samples; processing theaverage values of the errors with a Kalman filter to produce theboresight update quaternion; and combining the boresight updatequaternion and the boresight quaternion to provide an updated boresightquaternion.
 13. The method of claim 10, further comprising steps of:measuring a velocity of the slave sensor in the master reference bodyframe before the unit is launched; measuring changes in position of theslave system body frame in the slave system navigation frame in selectedtime intervals; and calculating a velocity error using the measuredvelocity of the slave sensor and the changes in position of the slavesystem body frame.
 14. The method of claim 13 wherein the calculatedvelocity error is used to update the attitude of the slave system bodyframe relative to the navigation reference frame to provide an improvedaccuracy of the slave system inertial measurement unit.
 15. The methodof claim 13 wherein the calculated velocity error is used in a Kalmanfilter to estimate error parameters of the slave inertial measurementunit and to apply corrections to improve its accuracy.